import rclpy
from rclpy.node import Node
from sensor_msgs.msg import NavSatFix,NavSatStatus
from geometry_msgs.msg import TwistStamped
from nav_msgs.msg import Odometry
from geographiclib.geodesic import Geodesic
import math
import numpy as np
from functools import partial


class GpsVelocityPublisher(Node):
    def __init__(self):
        super().__init__('gps_with_velocity_publisher')
        self.declare_parameter('ugv_ids', ['ugv1'])
        self.ugv_ids = self.get_parameter('ugv_ids').value
        self.origin_lat = 32.19925
        self.origin_lon = 119.50209
        self.altitude = 0.0
        self.earth_radius = 6378137.0
        self.geod = Geodesic.WGS84
        self.ugv_initial_gps = {}
        for i, ugv_id in enumerate(self.ugv_ids):
            offset_east = i * 3.0  
            g = self.geod.Direct(self.origin_lat, self.origin_lon, 90, offset_east)
            self.ugv_initial_gps[ugv_id] = (g['lat2'], g['lon2'])
        self.gps_pub = {}
        for ugv_id in self.ugv_ids:
            sub_topic = f'/{ugv_id}/out/odom'
            pub_topic = f'/{ugv_id}/gps'
            self.sub = self.create_subscription(
                Odometry,
                sub_topic,
                partial(self.odom_callback, ugv_id=ugv_id),
                10
            )

            self.gps_pub[ugv_id] = self.create_publisher(
                NavSatFix,
                pub_topic,
                10
            )

   
    def odom_callback(self, msg: Odometry, ugv_id: str):
        lat0, lon0 = self.ugv_initial_gps[ugv_id]
        x = msg.pose.pose.position.x
        y= msg.pose.pose.position.y
        #self.get_logger().info(f"Received Odom: x={self.x:.3f}, y={self.y:.3f}")

        distance = math.sqrt(x**2 + y**2)  # 米
        if distance < 1e-6:
            # 位移太小，直接使用原点
            gps_lat = lat0
            gps_lon = lon0
        else:
            azimuth = math.degrees(math.atan2(x,y))  # 注意：atan2(East, North) → 从北向东的角度
            # 使用 Geodesic.Direct 计算终点经纬度
            g = self.geod.Direct(lat0, lon0, azimuth, distance)
            gps_lat = g['lat2']
            gps_lon = g['lon2']
        gps_msg = NavSatFix()
        gps_msg.status.status = NavSatStatus.STATUS_FIX  # 表示定位成功
        gps_msg.status.service = NavSatStatus.SERVICE_GPS  # 表示 GPS 服务
        gps_msg.latitude = gps_lat
        gps_msg.longitude = gps_lon
        gps_msg.altitude = self.altitude
        gps_msg.header.frame_id = 'gps_link'
        gps_msg.header.stamp = self.get_clock().now().to_msg()
        self.gps_pub[ugv_id].publish(gps_msg)
       

def main(args=None):
    rclpy.init(args=args)
    node = GpsVelocityPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()